#!/usr/bin/python3
import serial
import time
import struct
import csv
import numpy as np
import threading
import psutil
import os
import sys

def crc16(data: bytes):
    """
    Calculate the CRC16-Modbus for a bytearray.
    """
    crc = 0xFFFF
    for byte in data:
        crc ^= byte
        for _ in range(8):
            if crc & 0x0001:
                crc >>= 1
                crc ^= 0xA001
            else:
                crc >>= 1
    
    return crc

TempBytes = []         # 临时数据列表
def DecodeData(data_frame: bytes):
    global TempBytes
    for val in data_frame:
        TempBytes.append(val)
        # 校验帧头
        # print(TempBytes)
        if (len(TempBytes) > 4):
            if (bytes(TempBytes[0:4]) != b'\xAA\xAA\x00\x18'):                   #非标识符0x55开头的
                # print("err", ' '.join(['{:02X}'.format(x) for x in TempBytes]))
                del TempBytes[0]                       #去除第一个字节
                continue
            elif (len(TempBytes)>=26):
                data = TempBytes[0:24]
                # print("unpack")
                received_crc = struct.unpack('<H', bytes(TempBytes[24:26]))[0]  # Assuming CRC is in little-endian format
                # print("CRC checking")
                crc_ret = crc16(data)
                # print("CRC checked")
                if received_crc == (((crc_ret & 0x00FF) << 8) | ((crc_ret & 0xFF00) >>8)):
                    # print(' '.join(['{:02X}'.format(x) for x in TempBytes]))
                    res = process_data(bytes(TempBytes))
                    # print(res)
                    timestamp = time.time()
                    IMU_DATA = np.append(res, timestamp)
                    # print(IMU_DATA)
                    with open(IMU2_data_file, "a") as f:
                        writer = csv.writer(f)
                        writer.writerow(IMU_DATA)
                    # 清空buffer
                    TempBytes = []
                else:
                    # print('{:04X}'.format(received_crc), '{:04X}'.format(crc_ret))
                    del TempBytes[0]
                    continue





def process_data(data):
    # 提取第7位到第12位的数据
    group1 = data[6:12]
    # 将数据分成3组,每组2个字节
    group1_data = [twos_complement(group1[i:i + 2]) / 100 for i in range(0, len(group1), 2)]

    # 提取第13位到第18位的数据
    group2 = data[12:18]
    # 将数据分成3组,每组2个字节
    group2_data = [twos_complement(group2[i:i + 2]) / 10000 for i in range(0, len(group2), 2)]

    # 提取第19位到第24位的数据
    group3 = data[18:24]
    # 将数据分成3组,每组2个字节
    group3_data = [twos_complement(group3[i:i + 2]) / 10000 for i in range(0, len(group3), 2)]

    # 将所有数据合并到一个数组中
    result = group1_data + group2_data + group3_data

    return result


def twos_complement(bytes_data):
    value = int.from_bytes(bytes_data, byteorder='big')
    if value & 0x8000:  # 检查最高位是否为1
        value = -((value ^ 0xFFFF) + 1)  # 进行补码转换
    return value

# 线程安全锁
serial_lock = threading.Lock()
def receive_data(serial_):
    # 先清空buffer
    serial_.reset_input_buffer()
    # 循环读取数据
    while serial_.isOpen() and recvthread.is_alive() :
        try:
            try:
                # 尝试获取锁资源
                serial_lock.acquire()
                tlen = serial_.inWaiting()
                if (tlen>0):
                    data = serial_.read(tlen)
                    DecodeData(data)
            finally:
                # 使用 finally 确保无论是否发生异常都能够释放锁资源
                serial_lock.release()
        except Exception as ex:
            print(ex)
        # 查看线程是否还在运行
        if not threading.main_thread().is_alive():
            print('IMU2 recv thread exit done')
            break
        

def Send_data(serial_):
    command = [0xAA, 0xAA, 0x00, 0x06, 0x05, 0x04]
    check_code = crc16(command)
    # check_code2 =crc16_ccitt(command)
    # print("校验",check_code,check_code2)

    command += [(check_code >> 8) & 0xFF, (check_code) & 0xFF]  # 将校验码加到命令末尾
    # print("command",command)
    # print("command",bytearray(command))
    # print(f"The check code1 is: {command:04X}")  # 输出为十六进制格式
    serial_.reset_output_buffer()
    while serial_.isOpen() and sendthread.is_alive():
        loop_start_time = time.time()
        try:
            try:
                # 尝试获取锁资源
                serial_lock.acquire()
                ser.write(bytearray(command))
            finally:
                # 使用 finally 确保无论是否发生异常都能够释放锁资源
                serial_lock.release()
        except Exception as ex:
            print(ex)
        # 查看线程是否还在运行
        if not threading.main_thread().is_alive():
            print('IMU2 send thread exit done')
            break
        if time.time() - loop_start_time < read_interval:
                time.sleep(read_interval + loop_start_time - time.time())

read_interval = 0.010  # 20ms

# 进程查询，防止多运行
def checkProcess(processName):
    # processName = os.path.basename(__file__)
    ps1=psutil.pids()
    curps=os.getpid()
    for pid in ps1:
        try:
            cmdline = psutil.Process(pid).cmdline()
        except psutil.NoSuchProcess:
            continue
        # print(cmdline)
        if processName in cmdline and pid != curps and pid != (curps - 1):
            print(f"{processName}:, This is the second process pid = {str(pid)}, so exit!")
            sys.exit(1)
    print(f"now runing {processName},current pid = "+str(curps))
    return

#主函数
if __name__ == "__main__":
    _fileName = sys.argv[1]
    checkProcess(_fileName)
    # 设置串口参数
    try:
        # ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=1)  # 请根据实际情况替换串口名和波特率,com7
        ser = serial.Serial('/dev/ttyS0', 115200, timeout=1)  # 请根据实际情况替换串口名和波特率,com7
        print("IMU_2 connect")
    except:
        print("error:  unable to open port .")
        sys.exit(1)
    # 构造命令
    Datafolder = 'Data/IMU2Data/'
    if not os.path.exists(Datafolder):
        os.makedirs(Datafolder)
    formatted_time = time.strftime('%Y%m%d%H%M%S',time.localtime())
    IMU2_data_file = Datafolder + f'IMU2_data_{formatted_time}.csv'

    with open(IMU2_data_file, "a") as f:
        writer = csv.writer(f)
        writer.writerow(["Pitch","Roll","Azimuth","magnetic X","magnetic Y","magnetic Z","ACC X","ACC Y","ACC Z","Time"])

    # 设置读取数据的时间间隔(以秒为单位)
    
    # 获取脚本启动时的浮点时间(秒)
    sendthread = threading.Thread(target=Send_data, args=(ser, ))
    recvthread = threading.Thread(target=receive_data, args=(ser, ))
    sendthread.start()
    recvthread.start()
    while True:
        try:
            if sendthread.is_alive() and recvthread.is_alive():
                time.sleep(1000)
                # print("主线程")
            else:
                ser.close()  # 关闭串口连接
                break
        except(KeyboardInterrupt, SystemExit):
            break
    